r"""
基本
----
    * 复位
    * 绕框
    * 黑小框
    * 任意放黑小框

发挥
----
    * 绿追红，距离小于三厘米
    * 绿追红，跟一起走框

此文件需要完成
------
    * 识别矩形框，得到矩形框的端点坐标
    * 通过串口发送矩形框的端点坐标

已放弃的目标
------
    * 识别激光点，得到激光点的坐标。
        原因：
        * 红色激光点在矩形框外的时候亮度比背景白纸高，而在矩形框内的时候亮度比背景低，导致无法通过颜色识别来识别激光点。
        *红色激光点和绿色激光点靠近的时候会结合成一个亮斑，导致识别点位出现偏差，产生这个现象的激光点间距在10cm以上，远远超出了题目要求

已完成的目标
    * 完成矩形识别器的封装
    * 封装串口发送，可以发送整型数组，包头0xff，包尾0xfe

*********************************************
                   _ooOoo_
                  o8888888o
                  88" . "88
                  (| -_- |)
                  O\  =  /O
               ____/`---'\____
             .'  \\|     |//  `.
            /  \\|||  :  |||//  \
           /  _||||| -:- |||||-  \
           |   | \\\  -  /// |   |
           | \_|  ''\---/''  |   |
           \  .-\__  `-`  ___/-. /
         ___`. .'  /--.--\  `. . __
      ."" '&lt;  `.___\_&lt;|>_/___.'  >'"".
     | | :  `- \`.;`\ _ /`;.`/ - ` : | |
     \  \ `-.   \_ __\ /__ _/   .-` /  /
======`-.____`-.___\_____/___.-`____.-'======
                   `=---='
									 
					佛祖保佑       电赛国一			 
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
"""
import struct
import time
import cv2
import numpy as np
import serial

def pack(func):
    """包头包尾修饰器

    * 包头：0xff
    * 包尾：0xfe"""
    head = 255
    tail = 254
    def wrapper(*args, **kwargs):
        ser.write(head.to_bytes(1, 'big'))     # 包头
        func(*args, **kwargs)
        ser.write(tail.to_bytes(1, 'big'))     # 包尾
    return wrapper

class RectangleDetector(object):
    """矩形检测器"""
    def __init__(self):
        self.Binarization = 113  #二值化参数
        self.canny_min = 0
        self.canny_max = 0
        self.offset = 9
        self.max_area = 20000
        self.min_area = 4000

    def __call__(self):
        # region 创建trackbar
        cv2.namedWindow('parameter', cv2.WINDOW_KEEPRATIO)
        cv2.createTrackbar('Threshold_0', 'parameter', 0, 255, self.call_back_Threshold)
        # cv2.createTrackbar('canny_max', 'parameter', 0, 255, self.call_back_Canny_max)
        # cv2.createTrackbar('canny_min', 'parameter', 0, 255, self.call_back_Canny_min)
        cv2.createTrackbar('adapt_thr_offset', 'parameter', 0, 255, self.call_back_offset)
        cv2.createTrackbar('min area', 'parameter', 0, 30000, self.call_back_minsize)
        cv2.createTrackbar('max area', 'parameter', 0, 30000, self.call_back_maxsize)
        # endregion

    # region 二值化回调函数

    def call_back_Threshold(self, Binarization_num):
        self.Binarization = Binarization_num

    def call_back_Canny_max(self, Canny_num):
        self.canny_max = Canny_num

    def call_back_Canny_min(self, Canny_num):
        self.canny_min = Canny_num

    def call_back_offset(self, adapt_offset):
        self.offset = adapt_offset

    def call_back_minsize(self, min):
        self.min_area = min

    def call_back_maxsize(self, max):
        self.max_area = max
    # endregion
        
    def Extract_the_edges(self, img):
        """提取边缘
        * img: 原图像"""
        img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        img = cv2.GaussianBlur(img, (5, 5), sigmaX=0.6)             #高斯滤波
        # cv2.imshow("2", img)

        ret, img = cv2.threshold(img, self.Binarization, 255, cv2.THRESH_BINARY)  #二值化
        # cv2.imshow("2zh", img)
        #img = cv2.adaptiveThreshold(img, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY_INV, 11, self.offset)
        # cv2.imshow("2zh ada", img)

        img = cv2.Canny(img, self.canny_min, self.canny_max)                       #canny算子
        # cv2.imshow("canny", img)
        return img


    def find_rectangle(self, img):
        """寻找矩形轮廓
        * img: 二值化图像"""
        approx = None
        point_sum = 0
        contours, _ = cv2.findContours(img, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)   #寻找轮廓

        for i in contours:
            area = cv2.contourArea(i)   #计算轮廓面积
            if area > self.min_area and area < self.max_area:
                img_new = np.zeros((480, 640, 3), dtype=np.uint8)
                approx = cv2.approxPolyDP(i, 0.02 * cv2.arcLength(i, True), True)

                cv2.drawContours(img_new, i, -1, (255, 255, 255), 2)
                cv2.drawContours(img_new, approx, -1, (0, 255, 255), 5)

                #print(approx[0], approx[1], approx[2], approx[3])

                # cv2.imshow("img_new", img_new)
                point_sum = point_sum + 1
        return approx, point_sum


class UART(serial.Serial):
    def __init__(self):
        super().__init__("/dev/ttyAMA0", 9600)

    @pack
    def send_arr(self, args:list):
        """发送数组"""
        for i in args:
            data = struct.pack('<i', i)     # 发送四个字节，端小字节序
            print(f'发送数据：{i}',end='')
            self.write(data)
        print()

    @pack
    def send(self, data:int):
        """发送整型数据"""
        newdata = struct.pack('<i', data)
        self.write(newdata)
    
    def __del__(self) -> None:
        return self.close()


class ColorDetector(object):
    """颜色识别器"""
    def __init__(self) -> None:
        """初始化"""
        # region 初始颜色阈值
        self.low_h = 0
        self.low_s = 0
        self.low_v = 0

        self.high_h = 180
        self.high_s = 255
        self.high_v = 255

        self.minarea = 0
        self.maxarea = 100000 # 220800
        # endregion

    def __call__(self,id):
        # region 创建trackbar
        cv2.namedWindow(f'trackbar{id}', cv2.WINDOW_NORMAL)
        cv2.createTrackbar('low_h', f'trackbar{id}', self.low_h, 180, self.call_back_low_h)
        cv2.createTrackbar('high_h', f'trackbar{id}', self.high_h, 180, self.call_back_high_h)
        cv2.createTrackbar('low_s', f'trackbar{id}', self.low_s, 255, self.call_back_low_s)
        cv2.createTrackbar('high_s', f'trackbar{id}', self.high_s, 255, self.call_back_high_s)
        cv2.createTrackbar('low_v', f'trackbar{id}', self.low_v, 255, self.call_back_low_v)
        cv2.createTrackbar('high_v', f'trackbar{id}', self.high_v, 255, self.call_back_high_v)

        cv2.createTrackbar('minarea', f'trackbar{id}', self.minarea, 100000, self.call_back_minarea)
        cv2.createTrackbar('maxarea', f'trackbar{id}', self.maxarea, 100000, self.call_back_maxarea)
        # endregion
        pass

    # region trackbar回调函数
    def callback(self, x):
        pass

    def call_back_low_h(self, low_h):
        self.low_h = low_h
    
    def call_back_high_h(self, high_h):
        self.high_h = high_h

    def call_back_low_s(self, low_s):
        self.low_s = low_s

    def call_back_high_s(self, high_s):
        self.high_s = high_s

    def call_back_low_v(self, low_v):
        self.low_v = low_v

    def call_back_high_v(self, high_v):
        self.high_v = high_v

    def call_back_minarea(self, minarea):
        self.minarea = minarea

    def call_back_maxarea(self, maxarea):
        self.maxarea = maxarea
    # endregion

    def detect(self, img):
        """颜色识别
        * img: 传入的图像数据

        返回值：二值化过滤后的图像数据"""
        _shape = img.shape
        try:
            hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)				# 将BGR图像转换成HSV图像
        except:
            return np.zeros((_shape[0], _shape[1]), np.uint8)

        low = np.array([self.low_h, self.low_s, self.low_v])		# 低阈值
        high = np.array([self.high_h, self.high_s, self.high_v])	# 高阈值

        mask = cv2.inRange(hsv, low, high)						# 通过阈值过滤图像，将在阈值范围内的像素点设置为255，不在阈值范围内的像素点设置为0
        kernel = np.ones((5, 5), np.uint8)						# 创建一个5*5的矩阵，矩阵元素全为1
        opencal = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)	# 开运算，先腐蚀后膨胀
        opencal = cv2.morphologyEx(opencal, cv2.MORPH_OPEN, kernel)	# 开运算，先腐蚀后膨胀
        return opencal

    def draw_rectangle(self, img, 
                       contours): 
        """在图像上绘制矩形框
        * img: 传入的图像数据
        * contours: 轮廓数据，通过cv2.findContours()函数获得的轮廓数据

        返回值：绘制矩形框后的图像数据，矩形框的坐标"""
        lst = []
        for cnt in contours:										# 遍历轮廓数据
            x, y, w, h = cv2.boundingRect(cnt)						# 获取矩形框的坐标和宽高

            if (self.minarea > w*h or w*h > self.maxarea):
                continue
            if h/w > 1.5:
                continue
            cv2.rectangle(img, (x, y), (x + w, y + h), (0, 255, 0), 2)	# 在图像上绘制矩形框
            lst.append((x, y, x + w, y + h))
        return img, lst
    
    def get_edge(self, img):
        """获取边缘
        * img: 传入的图像数据
        返回值：边缘图像"""
        # conuours是轮廓，hierarchy是轮廓的层级
        contours, _ = cv2.findContours(img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        return contours
    
    def expend_point(self, img, point:tuple[int, int], _expand:int=10) :
        """识别到激光点坐标后对其位置进行扩展，得到激光点周围的图像信息
        ------
        * img: 传入的图像数据
        * point: 激光点坐标
        * _expand: 扩展的像素数
        返回值: 激光点周围的图像信息"""
        _shape = img.shape
        # region 限制边界
        if point[0]-_expand < 0:
            x1 = 0
        else:
            x1 = point[0]-_expand

        if point[0]+_expand > _shape[1]:
            x2 = _shape[1]
        else:
            x2 = point[0]+_expand

        if point[1]+_expand < 0:
            y1 = 0
        else:
            y1 = point[1]+_expand

        if point[1]-_expand > _shape[0]:
            y2 = _shape[0]
        else:
            y2 = point[1]-_expand
        # endregion
        
        x1,x2,y1,y2 = map(int, [x1,x2,y1,y2])

        # 前面是x，后面是y，因为后面的return是先y后x
        nw = (x1, y1)       # 左上角坐标
        se = (x2, y2)       # 右下角坐标
        return img[nw[1]:se[1], nw[0]:se[0]]

    def __del__(self):
        """析构函数"""
        cv2.destroyAllWindows()
        pass

#摄像头初始化
cap = cv2.VideoCapture(0) #设备号为0

if cap.isOpened() == False:
    print('can not open camera')
    exit(0)

if __name__ == '__main__':
    ser = UART()
    # laserdetector = LaserDetector()
    rct_detector = RectangleDetector()

    rctLocation = None
    
    while True:
        _, img = cap.read()  # 读取图像
        img = cv2.rotate(img, cv2.ROTATE_180)  # 旋转图像
        frame = rct_detector.Extract_the_edges(img)
        rct_location, num = rct_detector.find_rectangle(frame)      # location 是矩形四个点的坐标，num是点的个数

        if rct_location is None:
            continue
        try:
            res = ((rct_location[i][0][0], rct_location[i][0][1]) for i in range(num))
        except:
            continue
        # print(res)
        res_lst = []
        for i in res:
            res_lst.append(i[0])
            res_lst.append(i[1])
        # region 串口发送数据
        if len(res_lst) == 8:
            print(res_lst)
            ser.send_arr(res_lst)
            time.sleep(5)
        # endregion
        # break
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

